#!/usr/bin/env python
import roslib; roslib.load_manifest('ebotMain')
import rospy
import math
from std_msgs.msg import String

class RecordWvels:

	def __init__(self):
		self.omega_l=0
		self.omega_r=0
		self.count=0

	def recordVels(self):
		rospy.init_node('recordWvels')
		rospy.Subscriber("wheel_vels", String, self.speed_avg)
		rospy.spin()

	def speed_avg(self,data):
#		try:
		ct = self.count
#		except UnboundLocalError:
#		ct = 0
#		print ct
#		try:		
		o_l = self.omega_l
#	except UnboundLocalError:
#		o_l = 0
#	try:
		o_r = self.omega_r
#	except UnboundLocalError:
#		o_r = 0	
		
		speeds = data.data.split(' ')
#		print 'Speeds: ' + speeds[0] + ' ' + speeds[1]		
		o_l += float(speeds[0])
		o_r += float(speeds[1])
		self.omega_l = o_l
		self.omega_r = o_r
		self.count = ct + 1
		if(ct == 200):
			lsp = o_l/100
			rsp = o_r/100
			print ('Left avg: ' + str(lsp) + ' Right avg: ' + str(rsp))
			quit()		
	
if __name__ == '__main__':
	rv = RecordWvels()		
	rv.recordVels()

